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ABSTRACT 


This research investigates the ability to create an undersea bathymetry map and navigate 
relative to the map. This is known as terrain aided navigation (TAN). In our particular 
case, the goal was for an autonomous underwater vehicle (AUV) to reduce positional 
uncertainty through the use of downward-looking swath sonar and employing TAN 
techniques. This is considered important for undersea operations where positioning 
systems such as GPS are either not available or difficult to put in place. There are several 
challenges associated with TAN that are presented: The image processing necessary to 
extract altitude data from the sonar image, the initial building of the bathymetry map, 
incorporating a system and measurement model that takes into consideration AUV 
motion and sensor uncertainty and near-optimal, real-time estimation algorithms. The 
thesis presents a methodology coupled with analysis on datasets collected from joint 
Naval Postgraduate School/National Aeronautical Space Administration experimentation 
conducted at the Aquarius undersea habitat near Key Largo, Florida. 
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I. 


INTRODUCTION 


A. MOTIVATION FOR THIS WORK 

A fundamental requirement for unmanned systems is the ability to accurately 
estimate position. The ubiquitous Global Position System (GPS) is used for a wide 
variety of aerial, surface and ground vehicles, but it has limitations—the signal can be 
jammed or occluded. What is desired is a robust methodology for position estimation that 
is not dependent upon an external system of navigational beacons. 

One alternative is terrain aided navigation (TAN). It is a technique that uses 
onboard, exteroceptive ranging sensors as a navigational aid. Seminal work developing 
TAN methods was first completed in the terrain contour matching (TERCOM) algorithm 
employed upon cruise missiles in the 1960s, before GPS was available. While the advent 
of GPS alleviated some of the motivation for further TAN work, it remained a viable 
option for undersea localization since GPS signals cannot significantly penetrate the 
water surface. 

Currently, commercial AUV systems rely heavily upon a costly, high-grade 
inertial navigation system (INS) in order to estimate the state of the vehicle. However, 
due to the dead-reckoning nature of INS systems, they are susceptible to drift over time. 
Unless localized by some other means, the vehicle’s positional uncertainty grows without 
bound. Typically, this growing uncertainty is corrected through the aid of either a 
network of acoustic ranging transponders or resurfacing for a GPS fix. Both methods are 
at the least an inconvenience, and at most are unrealistic, costly, and potentially mission 
threatening. TAN presents an appealing alternative method of localization for an 
underwater vessel that can be implemented real-time with sensors already onboard. 

B. ENABLING TECHNOLOGIES 

While TERCOM was first implemented in cruise missiles in the 1960s, the lack of 

necessary sensor accuracy, computational power, and data storage, along with other 

challenges associated with the undersea environment, have greatly delayed TAN 

implementation in the undersea domain [1]. Eor the last 35 years, the world has seen 
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remarkable improvements in computational power and data storage, as well as sensor 
accuracy. The increased processing power and data storage capabilities have not only 
significantly enhanced the accuracy of past, proven methods of TAN, but have also 
enabled the implementation of newer, more computationally expensive algorithms. 
A comparison of different methods for TAN is covered in Section D as well as in 
Chapter IV. 

C. PROBLEM DESCRIPTION 

TAN, unlike the related field of simultaneous localization and mapping (SLAM), 
requires a prior map of the region. The overarching goal of TAN is to effectively use the 
prior terrain map in conjunction with new sensor information in order to aid in the 
navigation of the vehicle. Therefore, the work presented in this thesis can be separated 
into two main subject areas: Building an accurate bathymetry map and using the built 
map as a navigational aid. 

First, a feature rich bathymetric map must be built. The map building process 
requires sonar image processing, a coordinate transformation between the vehicle’s body- 
fixed reference frame and a global frame, and a possible interpolation of the data points 
in the global frame. There are many challenges and considerations that must be made 
within the scope of building an accurate bathymetry map. One of the more significant 
concerns affecting map accuracy is the growing positional uncertainty of the vehicle as a 
function of distance. Other considerations include specific image processing techniques 
and threshold selections. For example, including the sonar response from a large fish in 
the map would not be a useful feature for subsequent localization. Ensuring abrupt 
changes in bottom topography are incorporated to the bathymetry map would be very 
important. 

The second main component of this work is implementing a method of TAN. 
TAN requires an algorithm that can effectively and accurately localize the position of the 
underwater vehicle based upon the previously built bathymetric map. There are several 
methods that can be used to varying degrees of success to accomplish this task. First, a 
kinematics motion model must be determined for the AUV. It is worth noting that the 
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kinematics for AUVs can be nonlinear, though they are often approximately well by a 
linearization [2]. Similarly, a measurement model must be formulated that encapsulates 
both the measurements and measurement noise. The exteroceptive measurements of the 
terrain are often highly non-linear as a result of the multiple peaks and valleys associated 
with the terrain. After appropriately modeling the vehicle and measurements, the TAN 
problem requires a correlation, or similarity measure, of the sensor’s current 
measurements with the prior bathymetry map. Some feasible similarity metrics include 
cross-correlation (XCOR), normali z ed cross-correlation (NXCOR), mean absolute 
difference (MAD) and minimum square distance (MSD) [1]. Due to the nonlinearity of 
the terrain, it was expected that the probability distribution resultant from the correlation 
would be multi-modal. TAN filtering methods currently being researched include 
Kalman-based filtering methods, multi-modal adaptive estimation techniques, and the use 
sequential Monte Carlo methods, namely point mass and particle filters [1]. 

In our application of TAN to an AUV, the vehicle built a bathymetry map of its 
environment using a micro-bathymetry swath sonar sensor. The sonar data was collected 
onboard the AUV vehicle and then post-processed in a MATLAB environment to “build” 
a bathymetry map. When the AUV subsequently traversed the same terrain, the new 
sonar sensor data it was receiving was correlated with the existing bathymetry chart in 
order to update the vehicle position estimate. Using only sonar data correlations (no state 
information from the vehicle), this can be an extremely expensive computational process. 
Therefore, several of the aforementioned methods that effectively fuse the knowledge of 
the previous vehicle state, the current sonar readings, and the existing bathymetry chart 
were explored in the scope of this thesis. These methods include the Kalman filter, the 
extended Kalman filter (EKF), and particle filter. A further glance into the advantages 
and disadvantages of each of these three methods is covered in Section D, and detailed 
more explicitly in Chapter IV. A primary motivation behind the eventual selection of the 
particle filter as a solution to the TAN problem was its ability to accurately estimate the 
probability distribution of the vehicle state, even when the distribution is multi-modal. 

This thesis is the first to address TAN on a small, man-portable AUV. The work 
is novel in the sense that it works with a bathymetry sonar sensor capable of significantly 
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higher resolution than most bathymetry sonar systems utilized for TAN. Along with 
concern to accommodate the higher resolution sensor, initial consideration has been given 
to image processing techniques and base map generation that are critical in an eventual 
real-time TAN implementation. This process was validated through simulation on a 
dataset collected by the AUV. 

D. RELATED WORKS 

The technical landscape of TAN methods is only now beginning to reach 
maturity. Until recently, very few commercial systems employed, or relied significantly 
upon, any terrestrial sensory information as a component of their navigation. As the field 
has developed, the TAN method of choice has shifted slightly. In the mid- to late-1990s, 
TAN methods for AUV’s employing Kalman filtering methods were primarily researched 
and implemented [3]. In particular, due to the aforementioned nonlinearities associated 
with TAN for an AUV, the extended Kalman filter (EKF) was a favorable method of 
sensor fusion among researchers. In the early 2000s, particle filters (PF) and point mass 
filters (PMF) were recognized as approaches well-suited to handle non-linear process and 
measurement models as well as multi-modal, non-parametric noise distributions. These 
techniques were selected, in part, due to the rapid increases in computer processing 
power/capabilities as PF and PMF are computationally expensive algorithms [4]. Several 
papers published throughout the early 2000s provide empirical support for the superiority 
of the PF relative to Kalman-based or batch-oriented methods. These papers include 
Gustafsson [5] in 2001, Nordlund [6] in 2002, and Anonsen and Hallingstad [7] in 2006. 

Carreno et al. [1] provide an excellent survey of AUV-based TAN research. 
Importantly, the survey paper also provides the Bayesian estimation framework relied 
upon throughout the TAN field. Bayesian estimation is a particularly useful approach in 
the underwater domain, since it implicitly accounts for the mean and variance associated 
with the pose of the vehicle. Approaches that use a Bayesian framework include Kalman 
filtering, multi-modal adaptive estimation and sequential Monte Carlo methods. 

The particle filter and point mass filter implementations by Anonsen and 
Hallingstad [7] on a HUGIN AUV provided very promising results demonstrating the 
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suitability of recursive Bayesian, sequential Monte Carlo methods to TAN. One large 
issue that is identified, but not addressed in their approach, is the impact of the terrain 
“usefulness.” Intuitively, flat terrain presents an issue when attempting to correlate the 
current measurements with the prior map. When the AUV was tested in suitable terrain, 
both methods yield positional accuracy comparable to their prior map resolution (10 m). 
However, it was noted that attempts to navigation in poorly suited terrain could often lead 
to filter divergence. 

In 2012, Professors Shane Dektor and Stephen Rock of Stanford completed 
further testing at Monterey Bay Aquarium Research Institute (MBARI) on the TAN of a 
Dorado-class AUV [8]. Similar to Teixeira et al. [9] they sought to improve the 
robustness of the particle filter TAN implementation. Incorrect convergence over time in 
flat terrain was determined to be a product of “overconfidence” of the filter in estimating 
position over the featureless terrain. In order to account for the overconfidence, an 
additional parameter dependent upon terrain suitability was incorporated into the 
correlation stage of the particle filter algorithm. The parameter effectively scales the 
observed correlation based upon the usefulness of the terrain being observed, thereby 
reducing correlation confidence over featureless terrain while maintaining the strong 
correlation and convergence of the filter over feature-rich terrain. 

E. THESIS ORGANIZATION 

The thesis is organized as follows: Chapter II provides a description of the AUV 
and the sonar used for research. After the system/sensor descriptions, the paper delves 
into building the bathymetry map necessary in order to conduct subsequent TAN 
operations. Only after the prior map building methodology is discussed in Chapter III is 
the selection of a TAN method discussed in Chapter IV. Finally, a dataset is collected and 
the methodologies described are implemented. The results and conclusions from the 
testing are presented in Chapter V. 
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II. SYSTEM DESCRIPTION 


A. REMUS AUV 

All data processing and experimental work was completed through the use of a 
modified REMUS 100 AUV (Figure 1) supplied by the Center for Autonomous Vehicle 
Research (CAVR) at the Naval Postgraduate School (NPS). The REMUS vehicles are 
currently designed and manufactured by Kongsberg Maritime 
(http://www.km.kongsberg.com). 

The REMUS 100 in particular is designed for operation in coastal areas in depths 
of up to 100 meters. This makes the REMUS 100 vehicle versatile in a variety of shallow 
water mission areas including, from [10]: 

• Hydrographic surveys 

• Mine counter measure operations 

• Harbor security operations 

• Environmental monitoring 

• Debris field mapping 

• Search and salvage operations 

• Scientific sampling and mapping 
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Figure 1. REMUS 100 AUV onboard CAVR’s SeaFox surface vessel 


The standard REMUS 100 is a lightweight and compact AUV, weighing 
approximately 37 kg and having a diameter of 19 cm. Its light weight and manageable 
size make it easily deployable and recoverable by two-man teams with small boats. The 
vehicle is modular and can be configured to employ a wide variety of sensors and 
systems. This includes: 

• MSTE side scan sonar 

• Upward and downward RDI acoustic Doppler current profiler (ADCP) 

Doppler velocity log (DVE) 

• Undersea acoustic modem 

• GPS 

• Fore and aft cross-body tunnel thrusters 

• YST600 Conductivity Temperature and Depth (CTD) sensor 

• Optical Backscatter Sensor 

• External Power Data Interface 

When at the surface, the REMUS 100 vehicle can obtain position fixes using 
GPS. When underwater, however, the vehicle relies on a long baseline (EBE), ultrashort 
baseline (USBE) or integrated DVE/GPS/INS Kearfott SeaDeViE navigation solution. 
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The SeaDeViL system has an advertised navigational uncertainty of 0.5 percent distance 
traveled circular error probable rate (CEPR) [12]. Essentially, this means that the actual 
position of the AUV will be within a circle of a radius quantified by 0.5 percent times the 
distance traveled. The EBE system has a navigational accuracy of +10 meters and range 
of 2000 meters. The USBE system has a navigational accuracy of +1 meter and range of 
1500 meters [12]. 

B. BLUEVIEW MB2250 SONAR 

As mentioned above, the REMUS has a modular external payload configuration. 
The power data interface provides a nominal 30 volts with an Ethernet and serial 
connection within a wet-mateable connector. This permits the REMUS AUV to mount 
forward-looking sensor packages. 

The forward-looking sensor package mounted on the NPS REMUS AUV for the 
data collected in this thesis is equipped with a BlueView 900 KHz forward-looking sonar 
and a BlueView 2250 KHz downward-looking, ultra-high resolution sonar. Both systems 
are blazed array sonars. A blazed array sonar system employs methods similar to 
echelette diffraction gratings in optics in order to turn a single sonar acoustic signal into a 
swath of sound beams [13]. Each beam is diffracted at a different frequency and therefore 
at a different angle relative to the source. The size and shape of each beam is dependent 
upon the frequency band of the original sound source as well as the shape of the stave 
diffracting the sound. Eigure 2 shows the magnitude and direction of each beam of a 
composite blazed array sonar along with the frequency range. 
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300 kHz 


600 kHz 



Figure 2. Composite blazed array sonar beam magnitude and direction for 
frequencies between 300 kHz and 600 kHz, from [13] 


As can be seen, the lower frequencies create larger beams and as the frequency 
increases, the beam size decreases. The frequency to spatial angle relationship remains 
consistent throughout the process. Therefore, once the sound from each beam reflects 
back towards the sonar system, the process is reversed and the individual beams are 
merged back into a single signal [13]. Due to the directionality of the individual beams, a 
wide swath of coverage can be attained with a high level of accuracy. For these reasons, 
most bathymetry mapping missions utilize a form of a blazed array sonar system. 

Table 1 provides the specifications of the 2250 KHz downward-looking, ultra- 

high resolution sonar used as the primary sensor in this thesis. 
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Table 1. BlueView MB2250 sonar specifications, after [14] 


Attribute 

Value 

Field of View 

45° X 1° 

Minimum Range 

0.5 meters 

Maximum Range 

10 meters 

Beam Width 

l°x 1° 

Number of Beams 

256 

Beam Spacing 

0.18° 

Time Resolution 

0.39 inches (0.01 m) 

Max Update Rate 

40 Hz 

Frequency 

2.25 MHz 
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III. MAP BUILDING 


The first step in the TAN process is the development of a base reference map. It is 
this map that is compared with bathymetry sensor measurements to determine the best 
correlation to determine the best position estimate for the AUV. This chapter describes 
the challenges and necessary steps for building the base bathymetry map. 

A. BATHYMETRY MAP BUILDING 

Constructing an accurate bathymetry map involves the following considerations. 

• AUV kinematics and dynamics and environmental impact 

• Limited sonar range 

• Low and asymmetrical frequency of sonar pings 

• Computational power constraints 

• Data storage constraints 

• Inherent inaccuracies associated with uncertain INS pose estimations 

One of the most challenging aspects of accurate map building is accounting for 
the positional uncertainty of the vehicle collecting the micro-bathymetry sonar images. 
Any navigational aid that can be utilized in order to constrain or reduce the uncertainty of 
the AUV is desired. This includes GPS, LBL and USBL systems. 

The growing, unconstrained uncertainty associated with the INS is by no means 
nominal. A conventional navigation pattern for complete sensor coverage is often called a 
“navigate by rows,” or “lawnmower,” mission. In the case of the REMUS vehicle, a 
typical survey area may be approximately 400 meters by 400 meters. Through setting the 
desired altitude of the vehicle at 9 meters, and given that the beam from the micro¬ 
bathymetry sonar is 45 degrees wide, the sonar swath will be 10.04 meters wide by the 
time it reaches the seafloor. Therefore, through using a spacing of 10 meters between 
each row, complete sensor coverage can be attained in ideal circumstances. In total, the 
AUV will travel approximately 16,800 meters. Given that the positional uncertainty is 
equal to 0.5 percent of the distance traveled without the use of any external navigational 
aids, the accumulated uncertainty by the end of the mission is 84 meters CEPR. 
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Figure 3 provides a visual representation of the accumulation of uncertainty as a 
mission progresses. The blue dots represent the mean estimate of the filter and the red 
ellipses represent the positional uncertainty associated with the INS navigation solution. 
Note that the positional uncertainty is drawn such that there is a 50 percent chance that 
the true position of the AUV is somewhere within the boundary. 

Mission parameters such as vehicle altitude, speed, and tightness of turns should 
also be given due diligence. A higher altitude off the seafloor inherently provides 
increased coverage area and thus lessens the needed distance traveled by the AUV in 
order to survey a specified area. The altitude is limited by the range of the sonar sensor 
(10 meters) plus a slight buffer of one to two meters in order to ensure abrupt changes in 
the topography do not cause a loss of bottom coverage. Similarly, given that the sonar 
system pings at roughly 1 Hz, additional coverage can be gained by decreasing the speed 
of the vehicle. Lastly, the dynamics and kinematics of the vehicle should be kept in mind 
in order to appropriately set realistic mission paths (i.e., turns wide enough that the 
vehicle is capable of making, or to set the minimum speed at which the vehicle should 
operate in order to maintain its depth and overall controllability). 
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Figure 3. REMUS AUV mission route with uneertainty—first navigate rows 

objective 


Figure 4 clearly quantifies the accumulation of navigational uncertainty 
throughout the course of the mission. The REMUS vehicle embarks on the mission with a 
GPS fix providing accuracy to within three meters. Approximately midway through the 
mission, the REMUS vehicle surfaces again in order to obtain another GPS fix. The GPS 
improves the INS solution and again confines the estimated position to a three-meter 
radius area. 
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Figure 4. Accumulation of REMUS positional uncertainty with GPS update 

Upon the completion of the mission, a final GPS fix is obtained and the positional 
uncertainty is constrained once again. It is clear from Figures 3 and 4 above that the 
vehicle must either surface periodically for a GPS fix or navigate its track with the aid of 
an acoustic navigation system in order to maintain a reasonable position estimate. 
Alternatively, the micro-bathymetry surveying may be completed using a surface vessel. 
This way, the vessel would have persistent GPS coverage. However, this method has its 
own limitations. For example, the high resolution BlueView MBE2250 sonar used in our 
experiments would severely constrain the operating area and coverage of the surface 
vessel due to its limited range. Typically, a sonar operating at a much lower frequency 
would be used in order to increase the sonar range. Using a lower frequency, however, 
negatively impacts the sonar image resolution. 

B. SONAR IMAGE PROCESSING 

Over the course of a mission, the AUV collects sonar images from its onboard 
sonar sensor. In a real-time implementation, image-processing techniques are applied 
onboard the AUV to provide a series of altitude measurements. For this thesis, the 
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analysis was conducted via post-mission processing. Upon completion of the mission, the 
raw images are then imported from the vehicle into a MATLAB environment in order to 
be post-proeessed. The goal of the sonar image proeessing step is to identify and extract 
information regarding the topology of the seafloor surveyed. 

A typical raw sonar image from the BlueView MB2250 micro-bathymetry sonar 
is shown in Figure 5. 


Micro-Bathymetry Sonar Image 
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Figure 5. Sample micro-bathymetry sonar image 

The intensity values of the image vary from 0 to 10000. A threshold of 125 was 
determined empirically to be a suitable threshold for separating noise from actual bottom 
returns. Thresholding attempts to minimize false aeceptances of noise as actual bottom 
returns while also minimizing false rejections, where valuable bottom information may 
be discarded as noise. It should be noted here that the thresholding value of 125 is the 

result of an empirical evaluation of a particular data set. It would be useful to include an 
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autonomous methodology for determining a filtering threshold regardless of the 
operational environment (i.e., sandy or muddy oeean floor). An adaptive thresholding 
method is a eonsideration for future work. For the dataset eolleeted for this thesis, a max 
return of every eleventh eolumn ean be loeated in the image and eompared with a 
threshold value of 125. If the maximum pixel value of the column is above 125, it is 
regarded as the bottom location in the image. Based upon sampling every eleventh 
column, a bottom return can be provided approximately every 0.25 meters. A map 
resolution of 0.25 meters was determined appropriate in seeking to accomplish the 
objectives set in this thesis. In future work, if determined desirable, the resolution could 
be increased to the fundamental 0.023052 meters/pixel associated with the sonar images 
produced. An example of finding the max return of each eleventh column in the sonar 
image is depicted in Figure 6. 


Micro-Bathymetry Sonar Image 



10000 


8000 


6000 


4000 


I 


2000 


100 150 200 250 300 

Pixels 


Figure 6. Sample sonar image with max returns 
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In the example of Figure 6, the four red pluses to the right of the visible bottom 
returns would be removed by the threshold from the eolleetion of useful bottom data. The 
final set of data points collected would be those represented by the red pluses in Figure 7. 
Using the pixel location of each point greater than the threshold along with the associated 
conversion of 0.023052 meters per pixel, the distance in meters to each max return can be 
computed in the vehicle’s local frame. 


Micro-Bathymetry Sonar Image 
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Figure 7. Sonar image with max returns after thresholding 


C. TRANSFORMATION TO GLOBAL FRAME 

Since the BlueView MB2250 sonar is rigidly attached to the underside of the 
REMUS 100 AUV, each sonar image is captured relative to the body’s pose. In order for 
the information to be useful, the bottom returns must all be represented in the same global 
map. The rotation transformation converts the identified bottom locations in the body 
frame to bottom locations in the local tangent plane (LTP) and converted into the global 
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frame. First, the vehicle’s pose is taken into account. The location of the bottom returns is 
affected by both the vehicle’s roll, pitch and yaw, represented by (j), 6, and y/ 
respectively. The Euler angle rotation matrix that rotates the information from the body to 
LTP frame is shown in equations (1) and (2). 
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( 2 ) 


All bottom returns from the micro-bathymetry sonar are collected in the AUV’s x, 
y, and z directions, using the conventional body-fixed coordinate system abiding by the 
right hand rule. In order to express all data points as relative to the ocean surface, the 
depth of the AUV must be added to the new z direction of the data points. The origin of 
the LTP is defined as a point on the surface of the ocean, and therefore a transformation 
between the vehicle’s body-fixed frame and the LTP necessitates both a rotation and 
translation of the reference frame. The rotation is determined by the vehicle Euler angles 
while the translation is determined by vehicle depth and an arbitrary x, y origin. An 
example of an AUV on a bathymetry mapping mission is shown in Ligure 8. 
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Figure 8. AUV with visible bathymetry sonar overlay, after [15] 


In practice, the body-fixed coordinate frame would originate at the vehicles center 
of gravity. The positioning of the coordinate frame in Figure 8 serves solely as a visual 
guide for the reader. Figure 9 depicts the very same image as Figure 8, but with red 
crosses symbolizing the maximum sonar returns associated with the seafloor. 



Figure 9. AUV with visible bathymetry sonar overlay and maximum bottom 

returns, after [15] 
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The coordinates of each red cross is first expressed relative to the vehicles body 
frame. After rotation and translation to the LTP, the same sonar returns seen in Figure 9 
would look approximately like the data points plotted in Figure 10. 


BJttmiiety Pont Cltuc 



Figure 10. Example sonar bottom returns expressed in LTP 


Since the AUV mission conducted for this project typically did not transverse 
more than several hundred meters across the seafloor in any direction, the LTP reference 
frame supports an acceptable representation of the data. Nonetheless, the points are also 
available represented in the global frame through latitude and longitude. The 
transformation from the LTP frame to the global frame was completed using MATLAB’s 
Map Toolbox. 

D. CONSTRUCTING THE BATHYMETRY MAP 

Once the entire set of data for the mission was transformed into the global 
reference frame in latitude and longitude, all of the points can be displayed coherently as 
a point cloud in 3D space. An example of several successive data points being displayed 
simultaneously in the LTP frame is shown in Ligure 11. 
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Bathymetry Point Cloud 



LTP Frame - West (m) 

Figure 11. Several successive sonar pings displayed together in the LTP frame 

During the correlation steps a sonar ping is rotated and translated into the LTP 
and compared with the base bathymetry map. The base map will likely not have complete 
coverage due to limited sonar resolution, AUV path planning and following constraints, 
and/or a slow sonar ping frequency among other reasons. A lack of complete coverage 
may result in an inability to make a comparison between the sonar ping and the base map 
as the sonar ping may ensonify an empty region in the base map. There are a number of 
possible approaches for handling the sparse data; however, this thesis provides complete 
coverage to a desired resolution using linear interpolation. The linear interpolation was 
performed in the MATLAB environment with the built in TriScatteredInterp function. 
The resultant surface passes through every data point in the set and linearly interpolates 
between neighboring data points. A basic linear interpolation may not best represent the 
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underlying topography and further research may be done in this area to identify a better- 
suited method of data interpolation. The linear interpolation of the sonar pings displayed 
in Figure 11 is exhibited in Figure 12. 



Figure 12. The linear interpolation of the sonar pings from Figure 11 


The same linear interpolation is applied to all data points collected within the 
survey area of the REMUS AUV. As one would assume, the linear interpolation better 
approximates the underlying topography in regions with a higher density of data points 
and is less accurate the sparser the data is. 
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IV. TERRAIN AIDED NAVIGATION 


This chapter addresses an optimal estimation approach to TAN. The goal is to use 
the prior bathymetry map from Chapter III as an aid to vehicle navigation. Therefore, the 
chapter starts with identifying a process and measurement model for the vehicle. The 
process and measurement model will be vital in the latter steps of filtering the positional 
estimation of the vehicle. Next, the correlation methods that quantify the similarity 
between the prior map and each new sonar ping are discussed and a method is selected. 
The correlation method coincides with the measurement model. Finally, the prior three 
components work together within the domain of a recursive Bayesian filtering method in 
order to estimate the vehicle state. Both Kalman-based filters and particle filters are 
discussed, and advantages / disadvantages for each are noted. Consistent with recent 
literature, and taking into consideration the nonlinearities and multi-modalities inherent 
to the TAN problem, a particle filter was chosen to be implemented. 

A. PROBLEM STATEMENT 

1. Process Model 

A generalized process model for an AUV is shown in equation 3, first proposed 

by [8]. 

= (3) 

where is the vehicle’s v, y, z position in the LTP and is representative of INS 
noise. The INS noise is assumed to be a zero-mean, white noise. 

2. Measurement Model 

The sonar and map models are shown in Equations 4 and 5 respectively [8]. The 
map and sonar errors, and are treated separately. Therefore, the sensor model 

uses the difference between the true terrain for the z'* ping at time k and the measured 
altitude for the z'* ping at time k along with a range dependent error, c,. ^, in order to 
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determine the range ^ . The map model essentially states that the true terrain range 
differs from the expected range h{xi^). by an error . 


yi,k=z,-h{x,).+e., 

( 4 ) 

h{x,)^=h{x,).+v^^^ 

( 5 ) 


• is the vector of the measured altitudes at each sonar ping. 

• yi ^ designates the ranges associated with the ping at time step k 

• h{x^). is the true terrain range for the ping at time k 

• h[x ^). range from a priori map 

• range dependent error e,., ~ N (O, ) 

• ^n.ap map error ~ N (O, ) 

Since the position of the AUV is not known precisely and the map has inherent 
errors, h(^Xi^). is not known. Therefore, effectively our goal with TAN is to minimize the 

argument by varying h{x^). throughout the search area. 

3. Correlation Method 

At each time step in the recursive algorithm, the altitude data from the micro¬ 
bathymetry sensor is compared to the prior bathymetry map at several locations. A 
correlation technique is necessary between the sensor data and the bathymetry map to 
quantify the similarity between each such that the best correlation is selected as the most 
probable location of the AUV. The correlation method should be robust across varying 
levels of depth as well as to sensor noise. Overall, the difficulties associated with map 
correlation are largely consistent with those of building the map originally. One of the 
significant difficulties associated with TAN is the low and asymmetric frequency of ping 
information. 
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Several different methods of correlation were tested including cross-correlation 
(XCOR) [16], normalized cross-correlation (NXCOR) [16], mean absolute difference 
(MAD) [1], a normalized minimum absolute difference, and a mean square difference 
(MSD) [1], 


NXCOR{x,) 


>ICOR{x,) = -Y,(z,j-Mx,)) 

■'V ( = 1 

i=l 

(=1 


( 6 ) 

(7) 

( 8 ) 
(9) 


In comparison testing with each method, NXCOR, MAD, and MSD all performed 
similarly well. XCOR was determined to be ill-suited to the problem as the correlation 
performed is not scale invariant, meaning the actual depth values from the prior map 
influence the result of the correlation. This is dangerous, as the correlation value between 
a sonar ping and itself may actually be poorer than a correlation between the same ping 
and an arbitrarily high valued set of sonar data. NXCOR successfully addresses this 
undesired dependence upon scale. The main downfall of NXCOR, however, was that the 
implementation only recovers Cartesian shifts in data. The MAD and MSD 
implementations, on the other hand, can recover rotation shifts as well as translational 
shifts. Both MAD and MSD performed similarly well, but due to the nature of squaring 
the differences, MSD exaggerates the peaks and valleys of the correlation matching. 
Therefore, MAD was chosen in order to avoid this sort of “overconfidence” in the 
correlation result. 


The output of the correlation step is a matrix equivalent to the size of the area of 

uncertainty searched. In the case of the MAD and MSD implementations, each cell’s 

value is the result of the correlation between the new ping and the prior map at that 

location. Through performing an element-wise inversion of each cell and then 

normalizing the whole matrix so that the sum of the matrix is equal to 1, a probability 

density function describing the probability of sensing the current ping information given 
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the AUV position in the map is calculated. This probability density function is key to the 
measurement update steps in the subsequent sections. 

4. Bayesian Methods 

As previously stated, given the dead reckoning nature of navigating by INS, there 
is a growing uncertainty associated with the vehicle’s pose over time. Clearly then, the 
TAN problem is best expressed through a stochastic process. Thus, the used in the 

process model of part 1 is not a single, precise v, y, z location, but a stochastic variable 
representing the estimate of the position. The well-known Bayesian filter represents a 
broad framework for which to estimate the a posteriori probability distribution of the 
vehicle’s state given a process model, measurement model, and the a priori probability 
distribution of the state. The Bayesian filter is represented in equations 10 and 11 as 
consisting of a prediction and correction step. The prediction determines the probability 
of the vehicle being at state given all of the previous measurements. This is done 

using the state transition model, represented by | , and the a priori 

probability distribution I-®*./) • The correction step then uses the result of the 

prediction step, along with the current measurement probability given the current state, 
represented by p{^y,^\x^) , and the probability of the measurement given all the previous 

measurements, represented by p (I D^_j ). 

PREDICTION :p{x, I D,_,) = \p{x,\ x,_„u,) p{x,_, I D,_,) (10) 

CORRECTION : p{x,\ DA = ^ (11) 

pyTk 

p ( Xi^_j ) Probability of robot at certain state (pose) at time step k -1 
P{^k\ ^k-i ’ ^k ) State transition probability (motion model) 

I ) Measurement probability - Probability of observing when at 
state x^ 


28 



Z)^ = {j,-: i = Dj^ ={y.:i = Set of all measurements up until time k 

The Bayesian filter above provides the general framework for which state 
estimation occurs. The Bayesian filter cannot be used directly; however, as there is not an 
analytical solution to the equations. Instead, recursive solutions to the Bayesian filter, 
such as the Kalman filter and particle filter, are potential methods for solving the 
Bayesian estimation problem. 

B. EVALUATION OF RECURSIVE BAYESIAN ESTIMATION METHODS 

The recursive Bayesian estimation methods that were evaluated in the context of 
this thesis were the Kalman-based methods as well as particle filtering. First, a brief 
summarization of the Kalman filter is provided. 


The Kalman filter relies upon a linear system of differential equations, typically 
expressed in state-space format as in equations 12 and 13, from [17]. 



(12) 

y,=Hx,+v, 

(13) 

Time update (prediction) 

xl = + Bu,_j 

(14) 

P, =AP,_,A^+Q 

(15) 

Measurement Update (correction) 

K,=P,H^(HP;H’+Ry 

(16) 


(17) 

P,={I-K,H)P- 

(18) 


where: 


Pf, —A posteriori error covariance matrix 
—A priori error covariance matrix 
Q —Process noise covariance matrix 
R —Measurement noise covariance matrix 
K —Kalman gain 

—Zero mean, white process noise ~ A^(0,Q) 

—Zero mean, white measurement noise ~ A^(0,/?) 

As partially evidenced by its formulation, the Kalman filter requires linear and 


Gaussian assumptions. The EKF has a largely similar representation; however, it is able 
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to complete the prediction step of the filter using a nonlinear set of differential equations. 
Therefore, the EKF is typically a better estimator than the Kalman filter for problems 
governed by a nonlinear process and/or measurement model. Nonetheless, the EKF still 
linearizes about the current mean and covariance at each time instance and requires the 
same Gaussian assumptions as the ordinary Kalman filter [17]. 

The particle filter is a different form of recursive Bayesian filtering. The particle 
filter discretizes the continuous probability density functions associated with Bayesian 
filter through a large set of particles [1], [18]. Each particle represents a potential state. 
The particles are distributed roughly according to the a priori and a posteriori probability 
density functions. As each particle represents a potential state of the vehicle, each particle 
is passed through a recursion of propagation (according to the process model) and 
measurement updates. The algorithm for a commonly used particle filtering method 
called a sequential importance resampling (SIR) particle filter, is shown in Figure 13. 

Ij Initialize - Approximate the a priori 
probability by a discrete number of equally 
freighted particles 

2J Collect new obsers'ation / measurement 

3) Compute importance weight according 
to the probability of obserx-ation given 
particle's state 

4) Normalize particle weights * • 

5J Resample fi'om particles according to | _[ 

weights (each new particle is equally- 
weighted) 

6) Propagate each particle to next time 
instance using process model 

7) Repeat steps 2-6 

•• 

Figure 13. SIR particle filter algorithm, after [19] 

The initialization of the particle filter is essentially a discretization of the 
probability density function at that time. In the case of the REMUS vehicle, an 
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initialization of the particle filter would be performed upon diving and would be based 
upon the uncertainty associated with the GPS fix just prior to diving. The next main step 
in the algorithm is related to the measurement update. The measurement update is 
performed by collecting the new measurement and weighting each particle according to 
the probability of being at that particular state given the new measurement. The weights 
for all particles are then normalized. Next is the resampling step. The same number of 
particles are drawn from the weighted particles, but are once again equally weighted. 
Once the particles have been resampled, the process model is used to propagate the 
particles forward to the next time instance. The filter then continues on to collect another 
measurement, weight the particles accordingly, resample, and propagate the particles for 
each time step. These steps can be done in real-time as the measurements/observations 
are being received. If there are no measurements at a given time instance, the particles are 
still propagated with the process model until a new measurement is available. 

One of the most advantageous aspects of particle filtering, especially with 
consideration to TAN for an AUV, is that it does not make any assumptions on the 
system model. While even the EKF makes certain linear and Gaussian assumptions, 
particle filter implementations do not. With a sufficient number of particles, nonlinear 
and multi-modal systems models can still be well-approximated. The main disadvantage 
of particle filters in comparison with Kalman-based filters is the significant increase in 
required computational processing power. Increased processing power is needed for 
particle filtering as typically 1,000-10,000 particles are required for an accurate 
approximation to the state probability density function [1]. Each particle must be 
propagated and updated at each time instance, and therefore computational demand can 
be significant. 

C. SELECTION AND APPLICATION OF METHOD 

Both Kalman-based methods and particle filters rely partially on measurement 
updates. In terms of AUV TAN, the measurements are the depth of multiple points on the 
seafloor beneath the vehicle. As terrain in general can vary greatly, with various 
mountains, valleys, peaks and dips, the measurements collected are highly nonlinear. 
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With such nonlinearity associated with the measurements, it is typically unclear exactly 
where within a region of terrain any single measurement may have come from. In other 
words, when correlating the measurements collected from a single ping with a prior map, 
especially given sensor and map error, there are likely multiple areas within the region 
that seem to correlate well. Over time, however, one such positional estimate will 
continue to correlate well while the others will not. It is due to the nonlinearity associated 
with terrain measurements and the resultant multinomial probability distributions that 
particle filtering was implemented vice a Kalman-based method. An example of the 
probability density function output from the correlation, or measurement update step, has 
been taken from a dataset described in the Chapter V results and is shown in Figure 14. 
Clearly, the distribution in the example of Figure 14 is binomial. With the potential for 
such multinomial distributions in mind, the particle filter was the preferential choice for 
filtering. 
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Figure 14. Probability of measurement given state— p( J* | ) 
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The sequential importance sampling (SIS) particle filter formed the foundation for 
most particle filters developed since [18]. The SIR particle filter is a later derivative of 
the SIS particle filter that differs in one key aspect; the resampling step. Through 
resampling, the particle filter can avoid a documented degeneracy phenomenon, where 
after a few iterations all particles have negligible weights except for one. Resampling 
essentially eradicates particles with small weights and instead concentrates on particles 
with larger weights. Additionally, the SIR particle filter requires minimal assumptions 
beyond the knowledge of the state dynamics and measurement functions [18]. The 
simplicity, popularity [1], and avoidance of the degeneracy phenomenon were key 
influencing factors in the decision implement the SIR particle filter as a TAN filtering 
method. 
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V. EXPERIMENTATION 


A. DATA SET COLLECTION—SEATEST II 

The primary source of data used for this thesis originates from cooperative 
experimentation entitled “SEATEST IT” The mission was conducted as a part of a joint 
experiment between CAVR at NFS and the NASA Johnson Space Center (JSC). The 
overarching goal of the collaboration with the JSC was to quantify the effects of 
autonomy on mission effectiveness, efficiency, and safety for joint robot-human 
operations [20]. 

The platform from which the cooperative experimentation took place is the 
Aquarius Underwater Research Station located in Islamorada, Elorida. The Aquarius 
underwater research habitat is the only operational underwater habitat in the world and is 
operated by Elorida International University (EIU). SEATEST II is similar to a series of 
exercises collectively known as NASA Extreme Environment Mission Operations 
(NEEMO) where astronauts train in the undersea habitat as an analogue to space 
operations. These “aquanauts” may be station at Aquarius for as long as a month at a 
time. The area of operations is visually represented by the Google Earth screenshot in 
Eigure 15. 
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Figure 15. Aquarius Research Station—Coast of the Florida Keys, from [21] 


CAVR contributed to the research effort with two REMUS 100 AUVs and a 
SeaBotix vLBVSOO Tethered, Hovering AUV (THAUS) alongside personnel with 
advanced technical backgrounds and aligned research objectives headed by Dr. Doug 
Homer and Dr. Noel Du Toit. The REMUS AUV is pictured in Figure 16 along with 
Aquanauts stationed at the Aquarius habitat (seen in background). 
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Figure 16. Aquanauts with the NFS REMUS AUV at the Aquarius Habitat, 

from [20] 

A research emphasis was placed on expanding unmanned system autonomy. 
Autonomy enables the vehicle, through exteroceptive sensing, to make intelligent 
navigational decisions such as obstacle avoidance and navigation in cluttered, dynamic 
environments. Underwater operations are of particular interest to NASA and CAVR 
research since these operations require unmanned system autonomy. The underwater 
domain not only necessitates the use of alternate means of positioning and navigation, as 
it is void of a ubiquitous positioning signal such as GPS, but also necessitates an alternate 
means of sensing and communication. Overall, the underwater domain presents a 
particularly unique and challenging environment for the operation of unmanned systems 
and as such. 

The missions conducted during SEATEST II combined autonomous mapping and 
navigation, multi-vehicle (heterogeneous) collaboration and information sharing, joint 
robot-diver operations, and persistent robotic operations. The role of the NPS REMUS 
vehicles was to survey a nearby area with a BlueView MBE 2250 micro-bathymetry 
sensor. Erom the surveyed sonar data, an accurate bathymetry map could be built. The 
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bathymetry map could then be used to identify an area of interest for subsampling by the 
REMUS or an aquanaut, or as a navigation aid to another AUV, such as CAVR’s 
SeaBotix vehicle. 

B. EXPERIMENTAL RESULTS WITH REMUS AUV 

I. Bathymetry Map Building 

Over the course of a particular REMUS mission collected during SEATEST II, a 
total of 3,046 sonar images were captured along with the estimated vehicle state at each 
instance. Of the 3,046 sonar images, 2,767 of them were within range of the seafloor and 
thus contained usable bathymetry information. Erom those 2,767 images, a total of 
56,491 data points were captured. The total distance traveled during the mission was 
approximately 6,250 meters. Eigure 17 provides a perspective on the location of the 
SEATEST II mission relative to the Aquarius underwater habitat in Islamorada, EE. The 
figure was composed using the data collected during the mission and importing into 
Google Earth as a KME file. Each yellow dot represents the position of the REMUS 
vehicle when a measurement is taken. 



Eigure 17. Bathymetry data points overlaid in Google Earth, from [21] 
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A GoPro camera was mounted underneath the REMUS vehicle just behind the 
BlueView micro-bathymetry sonar. Figure 18 is a snapshot from the video collected 
during the mission. The black cylindrical object at the top of the image is the forward- 
looking sonar / micro-bathymetry sonar attachment on the REMUS vehicle. 



Figure 18. GoPro image from SEATEST II mission 


Overall, the terrain was composed of rock and corral formations with intermittent 
sandy bottomed regions. The rock and corral provided noticeable variability in seafloor 
depth. Depth changes were not only apparent in each individual sonar image, but also for 
the dataset as a whole. All 56,491 data points collected within the survey area are 
expressed in the FTP as a 3-D point cloud in Figure 19. 
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Bathymetry Point Cloud 



LTP Frame - North (m) 


LTP Frame - West (m) 


Figure 19. 3-D bathymetry point cloud 

Figure 20 is also included in order to provide a different perspective of the data 
points collected. In Figure 20, the REMUS’s entrance and exit points from the survey 
area can be seen, but more importantly so can the density of the points collected. In 
particular, associated with each turning point is a gap of no coverage. It is important to 
note the density of the data points throughout the surveyed area since the linear 
interpolation better approximates the regions that have more data points. In regions where 
data is sparse, the linear approximation will likely be a poor representation of the 
underlying topography. Passes by the AUV over the areas interpolated by data points 
distant from one another can expect a poorer correlation between the sonar information 
being seen and what the interpolated prior map shows. This will negatively impact the 
overall performance of the TAN solution. 

For additional perspective on the dataset, the distance between each ping line is 
approximately two meters and the approximate size of the main survey area is 20,000 
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square meters. The distance between pings is due to the bathymetry sonar functioning at 
approximately ~ 0.5 Hz throughout the mission and the REMUS having a mission 
defined speed of 2 knots. 


Bathymetry Point Cloud 



LTP Frame - West (m) 

Figure 20. An overhead perspective of the bathymetry point cloud 

In order to provide complete coverage of the area surveyed, the bathymetry points 
were linearly interpolated to produce the results seen in Figure 21. 
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Interpolated Bathymetry Surface 


Figure 21. Interpolated bathymetry surface 


2. Terrain Aided Navigation 

Once the prior map of the surveyed area was constructed, it could be used as a 
navigational aid. Using the process and measurement models as a part of the particle 
filter, along with the output probability density function from the correlation step, an 
estimate of the vehicle state using the prior bathymetry map was calculated. Specifically, 
after the REMUS vehicle’s first pass in its survey area it has already accumulated a 
significant amount of positional uncertainty with regard to the INS positional estimate. 
An example of the magnitude of the uncertainty difference between the vehicles first and 
second passes through the survey area is depicted in Figure 22. The amount of 
uncertainty accumulated through the first pass of the region was approximately 0.670 
meters CEPR. By the time the vehicle passed through the area again, the positional 
uncertainty was approximately 8.068 meters CEPR. 
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Figure 22. Difference in uncertainty of INS positional estimation between 

subsequent passes of the same region 


Figure 23 is an example of the two-dimensional array represented as an intensity 
image using MATLAB’s colormap. Figure 24 includes red plus sign overlays on the 
image that represent the altitude measurements used for each sonar ping (after 
thresholding). 
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SEATEST II Sonar Image 



50 100 150 200 250 300 

Pixels 


1 8000 

7000 

6000 

- 5000 

- 4000 

- 3000 

- - 2000 

■ 

^■lOOO 

^®0 


Figure 23. Sonar image collected during SEATEST II 
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Figure 24. SEATEST II sonar image after thresholding data points 


In Eigure 24, it can be seen that 23 data points were extracted. The represents a 
swath width of approximately 5.75 meters orthogonal to the longitudinal direction of the 
AUV. All 23 points then went through a coordinate transformation based upon the Euler 
angles and depth of the vehicle in order to be expressed in the same global frame as the 
prior bathymetry map. Eigure 25 shows an example correlation between the 23 data 
points expressed in the global frame and the region of the prior map encompassing the 
vehicles current level of uncertainty. 
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Figure 25. Correlation probability distribution for a new sonar ping— p{yk I ) 

Figure 26 shows a sequenee of four eorrelation plots. This provides a snapshot of 
the multi-modality that typieally is assoeiated with sequential pings throughout the 
dataset. Each plot represents a probability distribution I that is output from the 
correlation step and used in the recursive Bayesian filter. 
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Figure 26. Correlation probability distributions for four different sonar images 
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The processing of each sonar image, transformation of the data to the LTP 
reference frame, and the computation of a probability distribution based on the 
correlation for the ping are all done in order to provide the measurement update for the 
particle filter. The measurement update and the process model for the propagation of the 
particles are the two main components of the particle filter. The results of several particle 
filtering simulations are shown below. 

The particle filter was first tested using 100 particles over the course of 54 micro¬ 
bathymetry sonar pings. Figure 27 is the result of this preliminary test of the particle filter 
algorithm. 
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Figure 27. Particle filter results on first leg of second pass of the survey area 

Figure 27 is indicative of the rapid convergence of the particles from an initial 
distribution with a larger standard deviation, to a much more tightly grouped set of 
particles. The initial standard deviation of the particles is 3.994 meters, while eight pings 
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later the standard deviation is approximately 0.5 meters. While the partieles at any state 
other than the initial state do not approximate Gaussian distributions, the standard 
deviation is still used here in order to provide a metric as to the spread of the particles. 

With the support of preliminary results, subsequent particle filter testing was 
completed using 1,000 particles. Figure 28 shows the results using 1,000 particles over an 
entire “navigate rows” mission objective through the survey area. 
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Figure 28. Particle filter with 1,000 particles compared to INS estimation 

Again, the filter can be seen to relatively quickly constrain the positional 
uncertainty of the AUV based upon the measurement and process updates. A GPS fix 
was taken at the end of the run and compared to the INS and particle filter estimations. 
The total positional uncertainty at the end of the run was 15.627 meters. The GPS fix was 
acquired using seven satellites with a horizontal error of approximately two meters. The 
INS solution as compared to the GPS fix was 14.272 meters away. The particle filter 
estimation as compared to the GPS fix was 8.863 meters away. 


49 






There are several error sources that can be attributed in varying degrees to the 
eventual particle filter estimation error of over 8 meters. GPS error, both from the 
initialization of the vehicle at the start of the mission and from the concluding GPS fix, 
could certainly account for a significant portion of the error. Additional error sources 
include the unaccounted for positional uncertainty in creating the prior bathymetry map, 
the linear interpolation of the prior map, and the particle filtering method. Within particle 
filtering, there can be a couple sources of error ranging from the selection of too few 
particles to properly approximate the probability density to the impact of various particle 
resampling techniques. Further literature research unveiled a detrimental effect of 
implementing a SIR particle filter on a vehicle with a high precision INS called “sample 
impoverishment” [2], [22], and [23]. A process model with small process noise, like that 
of the REMUS INS system, can be susceptible to a convergence on a false estimate. The 
particles are then propagated with small process noise and thus remain tightly clustered 
around a false estimate with little chance of recovery. This issue can be the result of a 
terrain change or artifacts in the prior map generation [22], or from premature 
convergence on a false estimate [23]. 

In order to address the concerns of sample impoverishment due to the small 
process noise associated with the REMUS INS, the particle filter was re-initialized 
shortly prior to surfacing for a GPS fix. Eigure 29 is the result from this simulation. 
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1000 Particles - Initialized shortly prior to GPS fix 
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Figure 29. Particle filter with 1,000 particles prior to GPS fix 


It can be seen in Figure 29 how the particle filter did not immediately converge to 
a single, tightly group position estimate but instead supported multiple hypotheses as to 
the location of the vehicle based upon the correlations being performed. Figure 30 is 
provided in order to clearly visualize the GPS positional update along with the final 
iteration of the particle filter using 1000 particles. While there clearly are multiple 
hypothesis as to the position of the AUV, the red “x” indicates the position of the best 
correlated particle. The best correlated particle is approximately 2.846 meters away from 
the GPS updated navigation estimate. 
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1000 Particles - Final iteration including GPS update 
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Figure 30. Final iteration of particle filter using 1000 particles 


The results from this simulation indicate that the relatively poor results from the 
testing of the particle filter on the full mission were substantially due to a false or 
premature convergence upon an estimate. Initializing the particle filter closer to the GPS 
fix produced significantly more accurate results. Further discussions on techniques that 
mitigate the chance of false convergence and sample impoverishment are discussed in the 
section on future work. 
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VI. CONCLUSIONS 


A. PERFORMANCE ASSESSMENT 

This thesis is the first to address terrain-aided navigation for a small, man- 
portable AUV. It included the entirety of the TAN process from building the prior 
bathymetry map as a navigational aid to the implementation and analysis of a particle 
filter for AUV position estimation. The combination of the ultra-high-resolution 
downward-looking sonar, INS and particle filter approach showed great promise for 
accurate underwater navigation. 

That said there are many areas for improvement. They can be categorized as 
follows: 

1. Map building 

a. Data interpolation 

2. Image processing 

3. Particle filter implementation 

4. Real-time implementation 

1. Map Building 

One of the most significant challenges in building an accurate bathymetry map is 
accounting for the inherent, growing uncertainty associated with the INS of the AUV. 
The problem poses quite a conundrum. Through TAN methods, one wishes to locali z e 
the position of the vehicle through the use of a prior map due to accumulating uncertainty 
with regards to position. However, the prior map is built using the INS solution, with its 
growing uncertainty, in the first place. Therefore, additional research is recommended 
into building a more accurate prior bathymetry map. A relatively simplistic fix to the bulk 
of this issue would be to survey an area using an LBL system. Although this would not 
eliminate the uncertainty with regards to the vehicle and thus the map, it would constrain 
it. Other potential methods of building a more accurate map could be the use of a quad¬ 
tree data structure that stores data different resolution grids based upon the certainty of 
the estimate or through ensuring consistency of the map by feature based correction 
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methods. This would entail a similar correlation technique as proposed above, but this 
correlation would be utilized in ensuring consistency with regards to the locations of 
features in the map. Overall, the vehicular positional uncertainty while building a 
bathymetry map should be taken into account, and methods for doing so are currently 
being researched. The fact that this was not taken into account in building the prior 
bathymetry map is a primary suspect for the poorer than expected accuracy of the particle 
filter. 


a. Data Interpolation 

As mentioned in the data interpolation paragraph, the TriScatteredInterp 
MATLAB function was used in order to perform a simple linear interpolation through all 
of the data points. A potentially more sophisticated method of interpolation (e.g., 
Kriging, Gaussian random fields or compressive sampling) takes into consideration the 
two-dimensional signal and the variance of the data may lead to a more accurate 
underlying map. 

2. Image Processing 

The sonar image processing algorithm utilized on this particular dataset worked 
efficiently and effectively. However, the algorithm is not robust to any significant 
changes in the environment. Poor performance could be expected in more dynamic, 
cluttered environments. In particular, testing in a harbor or kelp field would necessitate a 
more advanced and robust image processing algorithm. As a start, an adaptive threshold 
for identifying returns associated with the sea floor in various different environments 
with different bottom compositions would be useful. Additional work may be done to 
mitigate the effects of any noise that may appear in a dataset (noise was not a significant 
issue in the dataset collected). Nonetheless, the resolution of the data extracted from the 
image is only constrained by the fundamental pixel to meter ratio of 0.023052 and 
therefore the resolution could be increased at any point if it would be deemed worth the 
extra computational burden. More importantly, some changes on the image processing 
algorithm itself may be advised. 
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3. Particle Filter Resampling Technique 

As mentioned, the SIR particle filter implementation suffers from sample 
impoverishment. The sample impoverishment is largely attributed to the small process 
noise associated with the high-grade INS of the REMUS vehicle. A few methods that 
help counteract or negate this effect are discussed in [2], [22], and [23]. In particular, the 
Rao-Blackwellization of the particle filter as described in [2] is the recommended course 
of action for future work. Not only will Rao-Blackwellization help combat the effects of 
small process noise on the particle filter solution, but it will also reduce computational 
complexity and should improve accuracy [2], [5]. Other potential solutions include re¬ 
initializing the particle filter with a much large covariance when an error in estimation 
has been detected [22], using a genetic algorithm [23], or through the use of a different 
particle filter resampling technique such as the resample-move algorithm described in 
[24] or the regularization method described in [25]. 

4. Real-time Implementation on REMUS 100 AUV 

Perhaps most importantly, the end goal of this line of research is to implement a 
real-time system on CAVR’s REMUS 100 AUV. Concerns as to the long-term accuracy 
of the particle filtering solution should be addressed prior to fully trusting the navigation 
solution provided by the particle filter. Notwithstanding these concerns, the work 
provided thus far is suited to experimental testing on the REMUS vehicle. Real-time 
implementation would be the next major step in advancing TAN for an AUV. 
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